#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Kd_tree_d.h"

// Should be used with the Graph wrapper
struct Less_than_int
{
  bool operator()(const int& p1, 
                  const int& p2) const 
  {
    return (p1 < p2);
  }
};

class Prm
{
public:
	/*
		IMPORTANT:	The main code of the PRM algorithm should be placed in this file.
					You may change its structure, but make sure you that it remains consistent
					with the run() operation in Planner.h
	*/


	Prm (	int number_vertices, 
			int k_nearest, 
			CollisionDetector* col, 
			MultiRobotsSampler* sampler,
			ConfSet start, ConfSet target);

	~Prm();
	
	//	This operation is supposed to generate the roadmap (sample configurations
	//	and connect them.
	//	TODO: implement it.
	void			generate_roadmap();

	//	Returns a point path from start to target.
	//	If a path doesn't exist, returns empty vector.
	//	TODO: Implement this function.
	void			retrieve_path(vector<ConfSet>* path_vector_ptr);

private:
	void add_point_to_graph(Point_d p, int index);
	int get_point_index(Point_d p);
	ConfSet get_conf_set(Point_d point);


	int									m_number_vertices;	// number of sampled vertices
	int									m_k_nearest;		// maximal degree of vertex in roadmap
	CollisionDetector*					m_col;
	MultiRobotsSampler*					m_sampler;
	
	Graph<int>*							m_graph;			//	Graph structure
	Kd_tree_d<Kernel_d>*				m_kd_tree;			//	Kd-tree for nearest neighbor search

	ConfSet 							m_start,m_target;	//	Start and target configurations
	vector<Point_d>						m_all_tree_points;

};

#endif